#include <stdio.h>
#include <iostream>

#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/legacy/legacy.hpp>
using namespace cv;
using namespace std;

#include "PlaneNavigation.h"
#define SHOWIMG 1
#define CAMINDEX 1

//*****color division threshold value********
//read
#define  RedLowS  90
#define  RedLowV  90
#define  RedHighS  255
#define  RedHighV  255
#define  RedLowH  160
#define  RedHighH  180
#define  RedLowH2  0
#define  RedHighH2  10
//blue
#define  BlueLowS  43
#define  BlueLowV  46
#define  BlueHighS  255
#define  BlueHighV  255
#define  BlueLowH  100
#define  BlueHighH  124
//yellow
#define YLowS  40
#define YLowV  40
#define YHighS  255
#define YHighV  255
#define YLowH  20
#define YHighH  35
// step
#define Step_findblue 0
#define Step_fineyellow 1

extern "C" void updatePosition(CamPos * camPos, CP_MUTEXLOCK * mlock)
{
	float K[3][3] = { 349.176, 0, 307.576, 0, 350.889, 246.346, 0, 0, 1 };
	float P[5] = { -0.533113, 0.37188, 0.00411679, 0.00506721, -0.12096};
	Mat camerMatirx = Mat(3, 3, CV_32FC1, K);
	Mat disCoeffs = Mat(1, 5, CV_32FC1, P);
	VideoCapture cap(CAMINDEX);
	if (cap.isOpened() == false)
	{
		cout << "can 't open the camera" << endl;
		camPos->err_no = -1;
		return;
	}
	Mat image, ImgUndistort, hsvImage;
	Mat imageblue, imageyellow;
	Mat element_open = getStructuringElement(MORPH_RECT, Size(2 * 2 + 1, 2 * 2 + 1), Point(2, 2));
	Mat element_close = getStructuringElement(MORPH_RECT, Size(10 * 2 + 1, 10 * 2 + 1), Point(10, 10));
	vector<vector<Point> >  contours, contours_y;
	size_t max_blue, max_yellow, i, max_area = 0, tmp_area;
	RotatedRect brect;
	// init the parameter
	camPos->err_no = 0;
	camPos->flymode = 0;
	// begin main loop
	while (1)
	{
		cap.grab();
		cap.grab();
		cap.grab();
		cap.grab();
		cap >> image;
		// correct the image
		//ImgUndistort = image;
		undistort(image, ImgUndistort, camerMatirx, disCoeffs);
		// turn the BGR image format to hsv pixel format
		cvtColor(image, hsvImage, CV_BGR2HSV);
		// division the blue and yellow part
		inRange(hsvImage, Scalar(BlueLowH, BlueLowS, BlueLowV), Scalar(BlueHighH, BlueHighS, BlueHighV), imageblue);
		inRange(hsvImage, Scalar(YLowH, YLowS, YLowV), Scalar(YHighH, YHighS, YHighV), imageyellow);
		// open and close claculate
		morphologyEx(imageblue, imageblue, MORPH_OPEN, element_open);
		morphologyEx(imageblue, imageblue, MORPH_CLOSE, element_close);

		morphologyEx(imageyellow, imageyellow, MORPH_OPEN, element_open);
		morphologyEx(imageyellow, imageyellow, MORPH_CLOSE, element_close);
#if SHOWIMG
		cv::imshow("imgblue", imageblue);
		cv::imshow("imgyellow", imageyellow);
#endif // SHOWIMG
		// contours detect
		contours.clear();
		findContours(imageblue, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
		contours_y.clear();
		findContours(imageyellow, contours_y, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
		
		// find the biggest mark
		if (!(contours.size() || contours_y.size()))
		{
			// can't find blue mark
#if WIN32
			WaitMutexCP(*mlock);
#else 
			pthread_mutex_lock(mlock);
#endif
			camPos->isInitDet = 0;
#if WIN32
			ExitMutexCP(*mlock);
#else 
			pthread_mutex_unlock(mlock);
#endif
		}
		else
		{
			max_area = 0;
			for ( i = 0; i < contours.size(); i++)
			{
				tmp_area = contourArea(contours[i]);
				if (tmp_area > max_area)
				{
					camPos->flymode = 0;
					max_area = tmp_area;
					max_blue = i;
				}
			}
			for ( i = 0; i < contours_y.size(); i++)
			{
				tmp_area = contourArea(contours_y[i]);
				if (tmp_area > max_area)
				{
					camPos->flymode = 1;
					max_area = tmp_area;
					max_blue = i;
				}
			}
			if(camPos->flymode == 0)
			brect = minAreaRect(contours[max_blue]);
			else
			brect = minAreaRect(contours_y[max_blue]);
#if WIN32
			WaitMutexCP(*mlock);
#else 
			pthread_mutex_lock(mlock);
#endif
			camPos->isInitDet = 1;
			camPos->initPt.x = brect.center.x;
			camPos->initPt.y = brect.center.y; 
			camPos->initAngle = brect.angle;
#if WIN32
			ExitMutexCP(*mlock);
#else 
			pthread_mutex_unlock(mlock);
#endif
			//boundingRect(contours[max_blue]);
#if SHOWIMG
			circle(ImgUndistort, Point(camPos->initPt.x, camPos->initPt.y), 5, Scalar(0,255,0));
#endif // SHOWIMG
		}
#if SHOWIMG
		cv::imshow("undistort image", ImgUndistort);
		cv::waitKey(10);
#endif // SHOWIMG

	}
}
